#include "cx_control.h"
#include "stdio.h"
#ifdef CX_CONTROL_ENABLE_PID
#define PARAM(object) (object->param)
#define KP(object) (PARAM(object).kp)
#define KI(object) (PARAM(object).ki)
#define KD(object) (PARAM(object).kd)
#define TARGET(object) (PARAM(object).target)
#define INPUT(object) (PARAM(object).input)
#define PRE_INPUT(object) (PARAM(object).pre_input)
#define OUTPUT(object) (PARAM(object).pre_output)
#define INTEGRAL(object) (PARAM(object).integral_value)
float _cx_pid_handler_position(cx_uint dt, cx_pid_control_object_t *param)
{
    float err,pre_err;
    err = TARGET(param) - INPUT(param);
    pre_err = TARGET(param) - PRE_INPUT(param);
    INTEGRAL(param) += KI(param) * err;
    INTEGRAL(param) = (INTEGRAL(param) > PARAM(param).max_integral_value? (PARAM(param).max_integral_value):(INTEGRAL(param)));
    OUTPUT(param) = KP(param) *(err + KI(param)*err + KD(param)*(err - pre_err));
    PRE_INPUT(param) = INPUT(param);
    return  OUTPUT(param);
}
#endif

int main(void)
{
    cx_pid_control_object_t pid;
    pid.param.input = 1;
    pid.param.pre_input = 0;
    pid.param.kp = 10;
    pid.param.ki = 0.02;
    pid.param.kd = 1.2;
    pid.param.max_integral_value = 20;
    pid.param.pre_output = 0;
    pid.param.target = 100;
    pid.handler = _cx_pid_handler_position;
    printf("output:%f\n", _cx_pid_handler_position(10, &pid));
}